﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace RestService
{
    class KalmanSolver
    {
        // l~ = l(t-1) + u + w
        // P~ = P(t-1) + Qt
        // Qt = E[wtwt^T]
        // K = P~(P~ + R)^-1
        // P(t) = P~ - K*P~
        // l(t) = l~ + K(z -l~)

        //du bao vi tri
        //w = {1,0}
        //u se duoc thay doi theo thoi gian sau. hien tai dang xet di thang theo Ox
        public static double[,] l_predict(double[,] l, double[,] w)
        {
            double [,] u= new double [2,1]{{-3.34},{0}};
            double [,] lu = Matrix.CongMaTran(l, u);
            return Matrix.CongMaTran(lu, w);
        }
        //du bao hiep phuong sai P cua vi tri
        //P(0) = {10,0,0,10}
        //Q = {1,0,0,1};
        public static double[,] P_predict(double[,] P, double[,] Q)
        {
            return Matrix.CongMaTran(P,Q);
        }

        //Kalman Gain
        //R={1,0,0,1} 
        public static double [,] kalmanGain(double[,]P, double[,]R){
            try
            {
                double[,] pr = Matrix.MatranNghich(Matrix.CongMaTran(P, R));
                return Matrix.NhanMaTran(P, pr, 2, 2, 2);
            }
            catch (Exception)
            {
                throw;
            }
           
        }

        //Hieu chinh P(t)
        public static double[,] P_adjusted(double[,] Ppredict, double[,] KalmanGain)
        {
            double[,] pk = Matrix.NhanMaTran(Ppredict, KalmanGain, 2, 2, 2);
            double r1 = pk[0, 0];
            double r2 = pk[0, 1];
            double c1 = pk[1, 0];
            double c2 = pk[1, 1];
            return Matrix.TruMaTran(Ppredict, pk );
        }

        //Hieu chinh l(t)
        public static double[,] l_adjusted(double[,] lpredict, double[,] KalmanGain, double[,] z)
        {
            double[,] gainError = Matrix.NhanMaTran(KalmanGain, Matrix.TruMaTran(z, lpredict), 2, 2, 1);
            double r1 = gainError[0, 0];
            double r2 = gainError[1, 0];
            return Matrix.CongMaTran(lpredict, gainError);
        }

        public static double[,] ConvertToMatrix(double x, double y)
        {
            return new double[2, 1] { { x }, { y } };
        }
    }
}
